﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO.Ports;
using System.Threading;
using TCP;

namespace RixRoverControl
{
    public class ArduinoControl : RoverControl
    {
        private SerialPort port;
        private Server m_server;
        private Mutex m_mutex = new Mutex();
        private byte m_last_command;

        public ArduinoControl()
        {
            port = new SerialPort("COM3", 9600);
            port.Open();

            m_server = new Server();
            m_server.SetReceiveDelegate(OnReceiveCommand);
        }

        void RoverControl.update()
        {
            byte[] cmd = {0};

            m_mutex.WaitOne();
            cmd[0] = m_last_command;
            m_last_command = 0;
            m_mutex.ReleaseMutex();

            port.Write(cmd, 0, 1);
        }

        void RoverControl.close()
        {
            port.Close();
        }

        void OnReceiveCommand(byte[] msg, int num_bytes)
        {
            // get last command

            byte cmd = 0;
            for (int i = num_bytes - 1; i >= 0; --i)
            {
                if (msg[i] > 0)
                {
                    cmd = msg[i];
                    break;
                }
            }

            m_mutex.WaitOne();
            m_last_command = cmd;
            m_mutex.ReleaseMutex();
        }
    }
}
